/*
 * @Author: Mcfly
 * @Date: 2021-10-09 17:18:49
 * @LastEditTime: 2021-10-10 11:36:31
 * @LastEditors: Mcfly
 * @Description: 
 * @FilePath: \FTTfly\Filter\ButterWorth.c
 * 本人github:https://github.com/McflyWZX
 */
#include <stdlib.h>
#include <math.h>
#include "ButterWorth.h"

#define PI 3.1415926535f

ButterParameter* ButterWorth_Init(float sampleFrequent, float cutoffFrequent)
{
    ButterParameter *LPF = malloc(sizeof(ButterParameter));
    float fr = sampleFrequent / cutoffFrequent;
    float ohm = tanf(PI / fr);
    float c = 1.0f + 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm;
    if(cutoffFrequent <= 0.0f)
        //no filtering
        return NULL;
    LPF->b[0] = ohm * ohm / c;
    LPF->b[1] = 2.0f * LPF->b[0];
    LPF->b[2] = LPF->b[0];
    LPF->a[0] = 1.0f;
    LPF->a[1] = 2.0f * (ohm * ohm - 1.0f) / c;
    LPF->a[2] = (1.0f - 2.0f * cosf(PI / 4.0f) * ohm + ohm * ohm) / c;
    return LPF;
}

/*
函数名:	float LPButterworth(float curr_input,Butter_BufferData *Buffer,Butter_Parameter *Parameter)
说明:	加速度计低通滤波器
入口:	float curr_input 当前输入加速度计,滤波器参数，滤波器缓存
出口:	无
备注:	2阶Butterworth低通滤波器
*/
float ButterWorth_Work_LPB(float curr_input, ButterBufferData *Buffer, ButterParameter *Parameter)
{
    // if(Buffer->outputButter[0] == 0.0f 
    //     && Buffer->outputButter[1] == 0.0f
    //     && Buffer->outputButter[2] == 0.0f
    //     && Buffer->inputButter[0] == 0.0f
    //     && Buffer->inputButter[1] == 0.0f
    //     && Buffer->inputButter[2] == 0.0f)
    // {
    //     Buffer->outputButter[0] = curr_input;
    //     Buffer->outputButter[1] = curr_input;
    //     Buffer->outputButter[2] = curr_input;
    //     Buffer->inputButter[0] = curr_input;
    //     Buffer->inputButter[1] = curr_input;
    //     Buffer->inputButter[2] = curr_input;
    //     return curr_input;
    // }
    //加速度计Butterworth滤波
    //获取最新x(n)
    Buffer->inputButter[2]=curr_input;
    //print(Parameter.b)
    //print(Buffer.Input_Butter)
    //Butterworth滤波 
    Buffer->outputButter[2]=Parameter->b[0] * Buffer->inputButter[2]
                                                        +Parameter->b[1] * Buffer->inputButter[1]
                                                        +Parameter->b[2] * Buffer->inputButter[0]
                                                        -Parameter->a[1] * Buffer->outputButter[1]
                                                        -Parameter->a[2] * Buffer->outputButter[0];
    //x(n) 序列保存 
    Buffer->inputButter[0]=Buffer->inputButter[1];
    Buffer->inputButter[1]=Buffer->inputButter[2];
    //y(n) 序列保存 
    Buffer->outputButter[0]=Buffer->outputButter[1];
    Buffer->outputButter[1]=Buffer->outputButter[2];
        
    // for(int i = 0; i < 3; i++)
    // {
    //     if(isnan(Buffer->inputButter[i])==1)		
    //     { 
    //         Buffer->outputButter[0]=curr_input;
    //         Buffer->outputButter[1]=curr_input;
    //         Buffer->outputButter[2]=curr_input;
    //         Buffer->inputButter[0]=curr_input;
    //         Buffer->inputButter[1]=curr_input;
    //         Buffer->inputButter[2]=curr_input;
    //         return curr_input;
    //     }
    // }
    return Buffer->outputButter[2];
}
